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ABSTRACT 


This thesis presents the coordination of an unmanned, multi- 


vehicle team that navigates through a congested environment. A 





novel approach is outlined that enables the control of multiple 








vehicles based on both computer vision and optimal trajectory 


algorithms. Various sensors are used to achieve localization in 





the indoor environment in lieu of global positioning data. 


Specifically, a Quanser Qball quadrotor is equipped with a 








downward-looking camera and sonar altimeter, while a Quanser Qbot 





ground vehicle is outfitted with sonar and infrared range 
finders. This equipment is complemented by an Optitrack motion- 


capture system. 


Using conventional image-processing techniques, the bird’s- 


eye images supplied by the quadrotor provide information 





regarding the dynamic environment that surrounds the ground 





vehicle. The ground vehicle can then produce a global, optimal 


trajectory, assuring collision-free operations. The optimization 





problem is addressed by applying the Inverse Dynamics in the 
Virtual Domain (IDVD) method that uses both the inverse 


kinematics of the ground vehicle and obstacle information. 

















Furthermore, the IDVD method enables th separation of spatial 

















and temporal planning. As verification of the results of this 








research, the developed approach for path planning is executed in 





a fully controlled lab environment and then compared with a 


sonar-based, reactive obstacle avoidance technique. 
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a ae INTRODUCTION AND BACKGROUND 


A. GENERAL KNOWLEDGE 


Advances in science and information technology have 


brought about the advent of the unmanned vehicle, a powered 





platform that carries no operator and is either remotely or 





autonomously controlled. While unmanned vehicles have no 





single specific use or mission type, they are very 
versatile and can be used to complete a wide variety of 


tasks. The vehicles can b designed to be either 











recoverable or single-use, disposable devices. 


Moreover, unmanned vehicles can be organized into four 


major categories according to the medium through which they 





travel. These categories are: unmanned air vehicles (UAV), 
unmanned ground vehicles (UGV), unmanned surface vehicles 


(USV), and unmanned underwater vehicles (UUV). Military 





unmanned vehicles are often classified according to the 





mission type they are assigned. The number of these 
missions or uses is growing very quickly as leaders begin 


to appreciate the advantages of having a robot perform 








tasks that are considered dull, dirty, or dangerous for 


humans [1]. 


B. MISSION VARIETIES 





Unmanned vehicles are taking the place of humans in 
many different types of missions. The most recognizable of 


these tasks are military related. This notoriety is due to 








the exposure unmanned vehicles receive in the media. Some 





commonly known missions are mine and bomb 


detection/disposal, precision strike warfare, and 


information warfare. While military missions comprise a 


large part of all unmanned vehicle tasking, non-military 








applications of thes vehicles are also becoming more 








prevalent. Some of these missions include search-and-rescue 
operations, firefighting, and sea-bottom topography. Some 


of the most notable of all unmanned vehicle tasks, however, 





are intelligence gathering, reconnaissance, and terrain 
mapping. Prior to the use of unmanned vehicles, 
reconnaissance and terrain mapping required the use of an 


operator who would have to maneuver the vehicle for hours 





on end, and many times into hostile environments. The 


confinement and close quarters of thes vehicles often 

















caused operator fatigue. Because unmanned vehicles are not 
encumbered by the physiological limitations that accompany 
an on-board human pilot, they can be designed to maximize 


the amount of time a vehicle stays on-station. Unmanned 





vehicle pilots can exert control over the vehicle remotely 





and therefore can be swapped in and out as necessary. This 


capability enables a difficult mission to be completed ina 





safer and less-fatiguing manner. 


Cc. VEHICLE TYPES 


Lx Air, Ground, and Sea Vehicles 





Of all the categories of unmanned vehicles, UAVs have 


received the most research and attention. Recent successes 





in various global combat operations have created a demand 
for UAV technology and a competitive environment for UAV 
manufacturers. As a result, there are many different types 


of these aerial vehicles, boasting an impressive range of 





capabilities. UAVs can be rotary, fixed wing, or floating 
vehicles. There are many options in the UGV world as well. 


Z 





Thes vehicles can be propelled using tracks, wheels, or 


legs. Maritim vehicles are beginning to receive a 





Significant amount of attention and can either be surface 
or submersible vessels. For this research, which involves a 
cooperative mission between two vehicles, a three-wheeled 


ground vehicle and a quad-rotor were chosen becaus they 





each hold significant advantages over other vehicle types 











in their respective categories. 


2: Platform Selection 
a. Assessment of Quadrotor 


When comparing the quadrotor to other types of 
aerial vehicles, several distinct advantages and 
disadvantages come to mind. First, the quadrotor has the 


ability to hover in one location for an extended period of 





time. With regard to this research, this fact is a great 





advantage because the hover capability will help provide a 








slow, stable platform for the camera mounted on _ the 





vehicle. Additionally, the quadrotor’s ability to move 





directly in all three axes allows for maneuverability in 
tight, constrained places like what might be found in an 
urban environment. Moreover, the ability to hover allows 
for vertical take-off and landing, which frees mission 
planners from the operational constraints imposed by fixed- 


wing aircraft such as runways. Another benefit of 





guadrotors is their ability to move directly to an intended 





location. This enables quadrotors to follow trajectories 
that would be difficult or impossible for fixed wing 


aircraft. 


Quadrotors are also smaller and less-complicated 


than many other types of aerial vehicles. Low-power 
3 


processors, compact electric motors, and hi-density battery 
technology allow manufactures to build very small 
quadrotors. This enhances maneuverability and allows the 
vehicle to travel in compact spaces. Also, the counter- 


rotating blades of the quadrotor obviate the need for a 





tail rotor, which is necessary to counteract the torque 


produced by the main rotor in a standard helicopter 





configuration. Quadrotors maneuver by changing the 





individual speeds of the rotating blades. This fact 
eliminates the need for a complicated pitch-changing 


mechanism also used in helicopters. 


One of the greatest drawbacks to the quadrotor is 


its high energy consumption rate. Since the motors are 





continually operating, mission flight times are limited. 
Quadrotors, which are usually built smaller to increase 


maneuverability, have a small payload that restricts the 





types of sensors th vehicl can carry. These two 





limitations can play a role in constraining the types of 


missions available to the quadrotor. 


b. Assessment of Ground Vehicle 


The chosen ground vehicle for this thesis is the 





iRobot Create, a vehicle similar in size and shape to the 





more familiar iRobot Roomba. The vehicle, whose description 





will be covered in greater detail later, is circular with a 





wheel radius of 34 centimeters and height of 7 centimeters. 


One of the greatest advantages of this type of robot over 





other ground vehicles is its ability to turn 360 degrees 


while its center stays stationary. This fact, combined with 











the low height profile of the vehicle, enables it to travel 











in compact, tight spaces. Th vehicl uses a two-wheel 
4 





differential drive system with a third omnidirectional 





caster for balance. This uncomplicated system saves space, 


provides balance, and allows the motion of the robot to be 





easily programmed. Finally, the chosen ground vehicle is 


lightweight, which translates to longer battery life. 


While the iRobot is simple and has-~= many 
advantages, some of the features listed above are also 


limitations. For example, its low profile and small wheel 





radii limit the types of terrain the vehicl can 





transverse. Moreover, the smaller wheels combined with the 





differential drive system limit the speed of the vehicle. 


D. RELATED WORK 


1. General 





Autonomous vehicl research, and in particular, 
trajectory generation and computer vision based control, 
has become a very active area of research. The dramatic 
increase in successful autonomous vehicle military 


operations in the last several years has helped to 





incentivize both commercial and private research in the 





area. Moreover, coverage on media Internet sites such as 


YouTube and Facebook has contributed both familiarity and 





excitement to the field for the general public. 


2. University of Singapore 


The University of Singapore has a very active UAV 
research lab that has developed the “Lion” family of 
UAVs [2]. One member of this family, the SheLion (Figure 
1), is a small RC helicopter that has been modified with 
various add-on sensors such as a camera, sonar altimeter, 


and inertial navigation system. One of the most beneficial 
5 





features of the SheLion is that at has two 


processors [3]. One processor is dedicated to handling 





image processing operations while the other is. solely 





dedicated to the flight control of the vehicle. Because 
image processing is both time and computationally costly, 
by separating these tasks, the creators of SheLion are able 


to maintain safer and more efficient results. 





Figure 1: “SheLion” UAV and Virtual Model. From [3]. 


In keeping with the latest trend in small-scale UAV 
research, the University of Singapore has been devoting a 
Significant amount of time to vision-based navigation, 
surveillance, and tracking. Using the same UAV helicopter 


described above, a real-time vision algorithm was developed 





that uses feature extraction to identify likely targets and 








a simultaneously-running Kalman filter to estimate and 





predict the position of the target based on a motion 


model [4]. 


3:5 University of Pennsylvania 


One of the most visible quadrotor laboratories in the 
United States is at the University of Pennsylvania, or 
UPenn. Some of the most exciting features of the research 
being done at the General Robotics, Automation, Sensing and 
Perception Lab (GRASP) at Upenn can be seen on the video- 
sharing site, YouTube. The recorded videos, which can be 
found with a simple keyword search, boast millions of views 
and demonstrate quadrotors performing various aggressive 
maneuvers. The Grasp Lab has several UAVs which they use to 


demonstrate their control algorithms and real-time 





trajectories. One such UAV, the Hummingbird (Figure 2), 
built by Ascending Technologies, is shown performing flips 
and a wide array of complex trajectory maneuvers through 


obstacles and in formation with similar vehicles [5]. This 





micro-UAV is equipped with four passive optical markers and 
together with a 20-camera Vicon motion capture system is 
able to determine its location in the indoor lab 


environment ones The aggressive maneuvers of these 





quadrotors are possibl becaus the system actively 


switches controllers during different stages of movement. 





Figure 2: UPenn Micro-Quadrotor. From [6]. 


One of the key aspects of the research being done at 
the GRASP lab is the implementation of UAV formation 
flight. Using a leader-follower approach the micro-UAVs can 
execute predictive maneuvers and path following. 
Furthermore, in the event the leader is disabled, the 


remaining UAVs can estimate the downed vehicle’s virtual 





states [7]. 


4. Carnagie Mellon University 


Researchers at Carnagie Mellon University have also 


studied the idea of improving the performance of UGVs with 





global data collected from an aerial vehicle. One project 





involved using three dimensional overhead LIDAR data to 
help improve robot localization and global path 
planning [8]. The autonomous ground vehicle is a modified 
ATV that is equipped with LIDAR, FLIR, military-grade GPS, 
and stereo color cameras. One important feature to note is 
that the terrain data collected by the manned helicopter is 
downloaded prior to the start of the mission and is not 


provided real-time to the ground vehicle. The ground 





vehicl uses high resolution on-board sensors to help 


ensure the calculated trajectory remains collision free. 


5. DARPA 


The Defense Advanced Research Project Agency recently 





funded a collaborative effort between the Georgia Tech 


Mobile Robot Laboratory, the GRASP lab, and the University 





of Southern California’s Robotic Embedded Systems Lab to 
develop a system whereby a single human operator can 
control a team of aerial and ground robots to navigate 


through a small village while searching for human 





targets [9]. The aerial robots, Piper Cub J3- model 
airplanes, fly above the village taking high resolution 


aerial pictures while the ground vehicles, consisting of a 





modified Hummer truck and other assorted smaller vehicles, 
gather range data. The information is transmitted via an 


ad-hoc 802.11b wireless Ethernet network. An important 








feature to note is that this project includes an operator 
“in the loop” who is an integral component at many stages 
during the mission. 


E. MOTIVATIONS FOR RESEARCH 


Unmanned vehicles represent the leading edge of modern 








day technology. One of the driving motivations behind these 
systems is the benefit of removing a human operator from 
tasks that are tedious or dangerous. In the past, many jobs 


would require vehicl operators to stay on task for 





multiple hours, which introduces fatigue and ae higher 





likelihood for human error. Unmanned vehicles enabl 
mission organizers to have operators work in shifts to 


maximize on-scene time as well as performance. 





Additionally, since ther ar no operators on board the 
vehicle, unmanned systems can be used in environments that 


are dangerous to human life. 


The next step in unmanned vehicl research is to 





introduce a higher level of autonomy in the system. 





Autonomous systems reduce the need for human interaction 








with th vehicle, nabling operators to focus on other 





tasks. Moreover, autonomous systems can perform at a higher 
level than a human. On-board processors can collect and 


integrate data from multiple sensors and respond to this 





information faster than a human would be able to react. 
Removing the human element from the vehicle also reduces 
the need for larger, more armored systems. This reduction 


enables the lighter, more agile, autonomous vehicles to fly 





faster and more aggressively than the human body can 


tolerate. 
F. SPECIFIC RESEARCH GOALS 
While current unmanned vehicl systems nabl th 











exploration of remote, hazardous areas, they often requir 
a significant amount of human interaction, which is a drain 


on resources. Furthermore, many current autonomous ground 





vehicles ar designed for reactive navigation using 
feedback from sensors on the moving vehicle to avoid 


obstacles. This implementation is time-consuming as the 





vehicle must gather and process data as it travels. The 


goal of this thesis will be to design a system architecture 





that enables the control of a multivehicle team that 
accomplishes the same mission faster and more accurately. 


More specifically, a UAV will capture images of the 





obstacles surrounding the ground vehicle and extract the 
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location of these 


obstacle information will 
that will 


the 


generator calculat 








path for 


obstacles 


then 


ground vehicle. 


the 
be 


in global frame. This 


fed to a trajectory 


te the optimal, collision-free 


The process will then be 





implemented in a controlled 


feasibility of the method. 


lab environment to validate the 
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II. LAB SETUP 


A. LAYOUT 


The laboratory where all trials were conducted is 
designed so that all experiments can be performed in a 
controlled, safe environment. Moreover, the lab space can 


be reconfigured to adapt to the needs of the researcher. 





The equipment available in the lab consists of multiple 


Qball-X4 gquadrotors, Qbot ground vehicles, an indoor 





localization system, and two ground stations. 


The ground stations consist of PC computers running 
Windows 7 operating system with 3.20 Ghz processors and 
16 GB of RAM. These computers are positioned on the edge of 
the room and in front of this area is the operating space 
for the vehicles. The floor is covered by reconfigurable, 
interlocking rubber mats that are used to reduce glare and 


reflections that might cause interference with the 








Optitrack system. This rubber mat also helps protect the 


vehicles in the event of a system failure. 


All lab components can be operated via these ground 
stations which are equipped with the necessary software. 
Matlab / Simulink is the primary software in use during all 
trials. The lab uses QuaRC real-time control software as 


well as the OptiTrack Tracking Tools package that manages 





the OptiTrack camera system. Both of these programs are 


fully integrated with Simulink. 





Controlling the vehicles involves running at least two 





Simulink models on the ground station computer. The host 





model gathers data from the OptiTrack system as well as the 


USB joystick that can be used for manual control override. 
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The model transmits all data to the vehicles using an ad- 





hoc wireless network. The control models, which are linked 
to each specific vehicle, compile and download code to the 


Gumstix processors on board the vehicles. 


B. HARDWARE 
1. Qball1-xX4 
a. Introduction 


The Qball-xX4 quadrotor vehicle is designed and 
built by Quanser, a Canadian robotics manufacturing company 
that specializes in real-time control design. The vehicle 
has an open-architecture design which allows operators to 
test a variety of controllers ranging from basic flight 


dynamics stabilizers to advanced multi-vehicle trajectory 





planning and navigation algorithms. The vehicle features an 








on-board data acquisition system, an embedded computer, and 
a suite of sensors [10]. The quadrotor has a diameter of 


0.7 meters and a height of 0.6 meters. 


b. Protective Cage and Frame 


The Qball-X4 (Figure 3) is a unique quadrotor 


system becaus the vehicle platform is enclosed within a 








spherical, carbon-fiber cage. This feature is ideal for the 


indoor laboratory environment and helps ensure safe 








operation during tests that involve vehicles operating in 
close proximity. The cage is constructed from flexible 
carbon fiber rods that are inserted into rubber connectors 
which organize the rods into its spherical shape. The rods 
are stiff enough to provide support to the quadrotor when 


it rests on the ground, but are sufficiently flexible 





enough to absorb the shock of an impact. The rods, which 
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are easily replaceable, are designed either to break or 
become loose from the rubber connectors during a crash. The 
bottom of the cage is truncated slightly to provide the 


vehicle with a stable landing platform as well as place for 








the sonar altimeter. 


The frame of the vehicle is simply two aluminum 





cross-beams. The frame provides support to the vehicle 
components as well as to the cage, which is connected to 
the frame by rubber mounts to minimize any damage that 


would be caused by the shock of a collision. 


Qball-x4 


UNMANNED VEHICLE SYSTEMS LAB 





Figure 3: Qball-X4 Vehicle. From [10]. 


Cc. Data Acquisition Card/ Gumstix Processor 


The Qball-X4 is outfitted with a HiQ data 
acquisition card with an embedded Gumstix computer [10]. 


This assembly, which runs on a Linux-based operating 
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system, was designed by Quanser to enable vehicle control 


as well as sensor reading. The HiQ communicates wirelessly 





with the ground station to relay sensor outputs and receive 
code designed to be run on the Gumstix computer. The use of 
Simulink and the Quarc block set allows researchers to 


build models without intensive programming. Quarc is able 








to target the Gumstix embedded computer, automatically 








generate code, and execute the controller. The Input/Output 





of the HiQ Data Acquisition card (Figure 4) consists 
OF [10] 
° 10 pulse width modulated (PWM) outputs for motor 
control 
e 3-axis gyroscope, with range configurable for + 


75°/s, £+150°/s, +300°/s, resolution 











® 3-axis accelerometer, resolution of 3.33 mg/LSB 

@ 6 analog inputs, 12-bit, +3.3V 

e 3-axis magnetometer, 0.5 mGa/LSB 

e 8 channel RF receiver inputs 

e 4 Maxbotix sonar inputs 

° 2 pressur sensors (absolute and relative 
pressures) 

e 11 reconfigurable digital I/O 

e 2 TTL serial ports 

e Serial GPS input 
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Figure 4: Quanser HiQ Data Acquistion Board. From [11]. 


d. Motors, Propellers, Speed Controllers, and 
Power 


The Qball X-4 is outfitted with four E-flight 
Park 400 750Kv motors. Attached to each motor are a 10x4.7 





propeller (Figure 5) and an electronic speed controller. 





The speed controllers receive commands from the DAC board 
in the form of PWM outputs. The Qball-X4 is powered by two 
3-cell 2500 mAh Lithium Polymer batteries. These batteries 
provide a flight time of roughly fifteen minutes and must 
be maintained at charge above 10.6 volts to avoid permanent 


damage. 
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Figure 5: Motor, Propellor, and Speed Controller. 


e. Sensors and Communication 


The Qball is equipped with several sensors, 


however, not all of these were used in the control of the 





vehicle. For example, the magnetometer, which has a listed 
accuracy of 0.5 mGa/LSB, proved to be unreliable in the 
indoor laboratory environment. The inconsistency in this 
sensor’s measurements is most likely due to the large 


amount of unshielded wiring and the construction of the 





building. As a result, the gyroscope and accelerometer are 
the sensors chosen to control the roll, pitch, and yaw 
models of the vehicle. With regard to height control, the 


sonar altimeter is chosen because it provides’ very 





consistent measurements. The sonar used in this experiment 
is the Maxbotix XL-Maxsonar EZ3. This sonar takes readings 
at a 10 Hz rate and draws very little current. It has a 
range of 20-765 centimeters and a resolution of 1 cm [12]. 
The sonar is fixed to the bottom of the Qball cage so the 
vehicle pitch and roll must be accounted for in the height 


control model of the vehicle. Also, a correction must be 


Hee) 





made to account for the height difference between where the 
sonar is located and the center of the body-fixed 


coordinate frame. 


The Qball-X4 is able to achieve localization by means 
of the external OptiTrack motion capture system. The system 
uses light emitting diodes and infrared cameras to track 


the position of passive optical markers (Figure 6) placed 





on the vehicle. The localization system and method will be 


discussed in greater detail in a later section. 





Figure 6: Qball-X4 with Passive Optical Markers Attached. 


2. Qbot 
a. Introduction 


The Quanser Qbot (Figure 7) is an autonomous 
ground vehicle that consists of an iRobot Create and an 
array of upgraded sensors. The Qbot has been upgraded to 


include [13]: 


e 8 PWM outputs for servo motors 


Lo 


e 7 reconfigurable digital I/O ‘ports, plus. 1 





digital output LED 








° 7 analog inputs, 12-bit, +5V inputs, resolution 
6.2 mV 
e 5 infra-red (IR) sensors up to 150cm 
e 3 sonar sensors 15cm to 6.45m, l1-inch resolution 
e 3-axis magnetometer, resolution of 0.77 mGa 
e° USB camera up to 9fps color images 
e Wireless communications 
F 


tf 





Figure 7: Quanser Qbot. From [14]. 


b. Frame Design and Drive System 


The Qbot has a circular shape with a diameter of 
0.34 meters and a height (including camera) of 0.20 meters. 
With attached sensors, the unit weighs 2.95 kilograms and 
has a maximum speed of +0.5 meters/second. The vehicle has 
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three wheels, two differential drive wheels and one omni- 


directional caster wheel. The vehicle changes the direction 








of its movement by altering the speed of the individual 


wheels. 


Cc. DAC and Gumstix Computer 


The DAC aboard the Qbot is located underneath the 
black cover of the Qbot. Its primary use is to receive any 


analog or digital inputs that come from the attached or 





additional optional sensors. The PWM outputs can be used to 








drive servo actuators like those that might be found in the 
joints of a robotic arm. The Gumstix embedded processor on 


the Qbot is similar to that found on the Qball xX-4 





guadrotor. It is used to compile and execute the code that 
originates from the Simulink model built on the ground 
station. The code from the ground station and the sensor 
feedback from the Qbot are transmitted back and forth using 
the Wifi board that is connected to the Gumstix. 


d. Sensors and Communication 


The Qbot is equipped with a variety of sensors. 





There are three MaxSonar-EZ0O sonar sensors (Figure 8) that 
are connected to the analog inputs of the Qbot DAC. These 
sonars cover a range of 6 inches out to 254 inches with a 
resolution of 1 inch. The SHARP 2Y0A02 infrared sensor is a 


low cost range sensor that provides measurements in the 





range of 20-150 centimeters. The Qbot is also equipped with 
three “bump” sensors that register an analog 1 when the 


left, front, or middle of the vehicle bumper is pressed. 
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Figure 8: Infrared (Left) and Sonar Sensor (Right). After 
piS-]'. 


3. Optitrack Motion Capture System 


In the absence of a GPS signal, indoor laboratories 
must utilize a different type of localization system to 
track vehicle position and orientation. This laboratory 
uses the OptiTrack Infrared Camera system created by 


Natural Point Incorporated, a company which specializes in 





optical tracking solutions. There are 10 V100:R2 cameras 





that have the capability of tracking up to 32 rigid bodies 


[14]. The cameras were mounted along the ceiling in a 





manner to eliminate any blind spots in the camera capture 
volume. The capture volume is the region where the 
OptiTrack system can successfully track a passive marker. 
The cubes in Figure 9 represent the approximate capture 


volume in the lab setup used for this thesis. The pyramids 





represent the cameras. The capture volume for this lab is 





approximately 10 feet tall with a width of 12 feet and 
length of 18 feet. 
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Figure 9: OptiTrack Capture Volume. 


The V100:R2 camera (Figure 10) specifications are 


listed below: 


e Resolution-640 x 480 

e Frame Rate-100 FPS 

e Lens Field of View- 46 degrees 
e Interface- USB 2.0 

e Latency- 10 milliseconds 
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1.78 inches 1.44 inches 
45.2 mm 36.6 mm 


2.94 inches 
74.7 mm 





Figure 10: OptiTrack V100:R2 Infrared Camera. From 
[14]. 


Each OptiTrack camera is linked to the ground station 
computer via an OptiTrack USB 2.0 hub. The hubs each link 
up to five cameras, but each hub must also be connected to 
other hubs via cable to maintain camera synchronization. 


The OptiHub configuration is shown in Figure 11. 
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OptiHub Setup with 18 v100 R2 Cameras OPTISYNC 
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best to use the USB pertson the 
back of your systern, Thora ° 
should be twe separate groups of 
USE ports which could identity 

"4 


each EHO, Phag two hubs into 
one EHO and one hub into the 
other. See diagram to the right. 





USB Uplink 





Figure 11: OptiHub Connection Diagram. From [14]. 
Cc. SOFTWARE 
1 Quanser Quarc Toolbox 
Quanser Real-Time Control (QuaRC) software enables 


rapid control code prototyping and hardware in-the-loop 
testing. Using QuaRC, a vehicle controller can be built in 
Simulink and converted to real-time code that can run on 


many different target processors. This method allows 





researchers the opportunity to focus on the controller 


design process without becoming mired in low-level 





programming. Additionally, an added time-saving benefit of 


the QuaRC software is that many control parameters in the 
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model can be adjusted while the code is running. This 
allows rapid testing and controller tuning without having 


to recompile the model with each alteration. Another 





advantage of using the QuaRC software suite is that 
multiple controllers can be run simultaneously on the same 
processor. This fact means that one ground station can 


operate multiple vehicles simultaneously. Since models are 





run in external mode, real time data can be displayed using 
scopes and displays, an important feature for vehicle 


operators. 


2. Tracking Tools Software 


The OptiTrack Tracking Tools software package is fully 
integrated with Simulink and the QuaRC toolbox. The QuaRC 








OptiTrack block set provides the user with the capability 
of tracking numerous passive optical markers simultaneously 
in 3-D environment. One of the advantages of the OptiTrack 
system is that it can be calibrated in roughly five 
minutes. The calibration process is very simple and only 
involves the use of two tools, a trident with passive 
optical markers (Figure 12) on the tips and an L-shaped 


tool that is used to mark the zero point of the room. 


The process involves first performing a visual check 


of each camera view to nsur ther ar no false 





reflections from objects in the camera field of view. If 
the reflecting object is not easily removable from the 


workspace, then the software allows one to place a virtual 











mask over this reflection. After this check is complete, 





the user begins “wanding” by moving the trident in a 
figure-eight pattern throughout the entire lab workspace. 


During this process, the software provides an indication to 
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the user about the general quality of the calibration. 


When the desired quality is achieved, the final step is to 





place the L-shaped ground plane tool on the ground in the 


center of the capture volume. The software then sets this 





as the origin from which all measurements will be based. 


The calibration is them saved to a file which must be 





referenced in the ground station host model that provides 
the localization data to each vehicle. Another important 
feature of the Tracking Tools Software is the ability to 
create “trackables.” These are unique arrangements of 
passive optical markers that can be fixed to vehicles in 
order to track not only the vehicle’s x-y-z position, but 


also the angular orientation of the vehicle. 





Figure 12: Ground Plane Tool (left) and Wanding Tool 
(right). 
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III. VEHICLE MODELING AND CONTROL 


A. INTRODUCTION 


Prior to performing any obstacle avoidance or 
trajectory generation, the motion and control of the 


vehicles must be understood. Feasible collision-avoidance 





trajectories requir knowledge of the physical parameters 


and correct control inputs for the vehicle. In this 





chapter, the simplifying assumptions about the operating 
environment and vehicles will be outlined. This will be 
followed by a section about coordinate frame designation 
and sections about the control and modeling of the Qball-X4 
and Qbot. 


The modeling of the vehicles is outlined according to 


state space format. This means that the dynamics of each 





vehicle is contained in a set of differential equations 





which is represented in matrix format. Certain assumptions 





are made and linearization about a point occurs. 


1. Assumptions and Simplifications 





Ther are several assumptions that can be made to 


Simplify the complexity of the Qball-X4 and Qbot models: 
e The Earth is a flat, non-rotating surface. 


e Farth is the only body that exhibits a 


gravitational force and the acceleration due to 


m 
this force is a constant 9.81 er 
S 


e The Quadrotor and Qbot have rigid bodies that do 


not flex. 
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e Drag forces are negligible due to slow vehicle 


speeds. 
e Quadrotor pitch and roll angles are small. 


e The Qbot is symmetric about its centerline axis 
while the quadrotor is symmetric about both its 


pitch and roll axes. 


2. Coordinate Systems 


With the exception of the image processing coordinate 
frames that will be discussed later, this thesis deals with 
two main coordinate frame types. The first type is the body 
fixed frame. There are two body fixed frames (Figure 13) 
that attach to the centers of each vehicle and rotate with 
them. An X-Y-Z Cartesian coordinate system is used for both 


body fixed frames. 





Figure 13: Body Fixed Coordinate Systems. After 
pale ease 3 Bee 


As shown in Figure 13, the X axis aligns with 
direction of forward movement, the Y axis points to the 
left, and the Z axis points up. All coordinate systems used 


in this thesis are right-handed coordinate systems. The 
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other type of coordinate system that is used is a local 
tangent plane (LTP) coordinate system. This coordinate 
system is used by the OptiTrack system as a reference 
coordinate system for the vehicles. The LTP serves to 


approximate the Earth as a flat, non-moving object. This 





approximation is possible because neither the Qbot nor the 








Qball operate at fast speeds or travel long distances. The 








effects of the Earth’s curvature and sidereal motion can be 





ignored. The X-Z plane is placed on the floor of the lab, 





while the Y axis point upward. 


B. QBALL-X4 QUADROTOR 
a Motor Control and Thrust Modeling 


A quadrotor is a type of rotorcraft that is propelled 











by four symmetrically-pitched, but independently controlled 





rotors. Generally, control of the vehicle is achieved by 








altering either the pitch of the blades or the individual 








rotational rates at which they each turn. As discussed 
previously, the Quanser OQball-xX4 does not have any 
complicated pitch-changing mechanisms so the individual 
rotor thrusts control the movement of the vehicle. In order 


to keep the vehicle stable along the yawing axis, the four 





rotors are organized into two sets of counter-rotating 


pairs (Figure 14) along the X and Y axes. 
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Figure 14: Quadrotor Motor Dynamics. From [15]. 


The specific thrust that is generated by each 


propeller is modeled by the first order system equation 


ay ee 





u, (1) 
S+@ 


thrust,i 


In this equation, u, represents the PWM input of the motor, 


@ is the bandwidth of the motor, and K is a positive gain. 





Oo 
The state variable vis K u; and this is used to 
5 


represent actuator dynamics. The control inputs for each of 


the specific controllers are defined as: 


F, + F, + Fi, + Fy, 


1 
m 


U, =F, —F, (2) 
U,=F, —F, 
U,=(F, + F, -F,-F,)*d 
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The value, d, is the force to moment scaling factor that is 
dependent on various blade parameters such as Reynolds 
number, Mach number, and angle of attack. Some necessary 


system parameters are tabulated as: 



































Parameter Value 

J 0.03 kg-m 

J. 0.04 kg-m 

K 120 N 

K, 4 N-m 

L 0.2 m 

M 1.4 kg 

o 1S rads 
Table 1. System Parameters. From [16]. 
2. Roll, Pitch, and Yaw Models 


Roll and pitch of the quadrotor occur around the X and 


Y axes respectively. An ideal feature about the Qball is 








that it is symmetric about its X and Y axes. This implies 














chat: the, moments: of inertia J,=s, J; 2 facet chat 


simplifies the dynamic equations for the second derivatives 
of roll and pitch. Also, because hover flights do not 


require large pitch and yaw angles the following 





simplification can be made: 





g << 0.1 > sin(@) = 0, cos(@) = 1 
0 << 0.1— sin(@) = 0,cos(@) =1 


Having made these simplifications, the roll, pitch, and yaw 





rates can be stated as: 
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JG a U,l 
J,,0 =U (4) 
JW = U, 


Using Au=u,—-u, or Au=u,-u, we can establish state space 


equations for roll and pitch models (Equations 5 and 6 





respectively). 


r-7 [0 1 O 

é KI ie 

¢)/=|0 0 = b\+| 0 |Aw (5) 
ee 10 0 -o| y 7) 
Pad ee . 6| [0 

6\=|0 0 ra 6 |+| 0 |Au (6) 
Pelt ihGe (Oy seplterdt -EM 














As a result of the symmetry of the Qball-X4, the pitch and 
yaw controllers are very similar. The controller is a 
linear quadratic regulator (LOR) that appends a fourth 


state S=@ and s=@to allow for integrator feedback. 
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The implemented controller is in the state-space form 





X=Ax+Bu where x is the state variable, A is the state 
matrix, and B is the input matrix. The controls given in 
the form u=-kx. An LOR controller works by minimizing a 


cost function with weighting factors determined by the 





control designer. The feedback gain, k, that results from 


these inputs results in poles at 


-19.827, -4.0834+4.275i, -4.083-—4.275i, -0.316 and can be determined 





from the weighting matrices Q and R. 


100 0 0 0 
00 0 0 
2) 60 22000 0 
0 0 O 10 
R = 30000 (9) 


With respect to the yaw model, the torque generated by each 


motor iS proportional to the PWM input of that respective 





motor. As shown previously, JW =U,=K, Au where 


Au=u,+u,+u,+u,. In state-space form: 


0 
1 
W wl] |= 


J 


zz 


The LOR controller for the yaw axis was designed in the 


same manner as the pitch and roll controllers. The 


[1 0 
O= 0 01 (ah) 


weighting matrices are: 


3. Position Model 


The quadrotor changes its location in the X-Y plane by 

















altering its pitch and roll angle. This is apparent becaus 


when the quadrotor has a non-zero pitch or roll angle, the 





thrust vectors from the rotors have a horizontal component. 
Once again, assuming small pitch and roll angles, xX and jy 


have the following state space equations. 


eee ar 
ie (12) 
yx—-—4kKv¢ 
m 
ee (Ol Oe Olea ing 
X| |0 0 ay oll x} {0 
f= m + u 
PY HOO wm. OI | 
Pe icc: Age, ge ee 
01 0 01, ver 
ly ly] [Oo 
Y| |0 0 aay OY] | 0 
= m + u 
1 OO. So OI | 
lel yy oo obs LY 











Once again, the controller was designed using LOR methods 


on Matlab with poles at -6.712,—1.614+0.792i,—1.61-—0.792i,-0.142 and 





with weighting matrices given by: 


36 


ooo oO 
- Oo Oo Oo 


Co ON CO 


R=50 (14) 


4. Height Model 


Vertical motion is caused by the vertical component of 





thrust provided by the rotors and can be modeled as: 


Mz = 4F cos(@) cos(@) — Mg (15) 


F, M, and g are the thrust from the propellers, the mass of 
the quadrotor, and acceleration of gravity respectively. 


The height controller, unlike the previous yaw, pitch, and 





roll controllers, is a proportional, integral, derivative 





(PID) controller. A PID controller calculates an error 





value as the difference between a measured variable anda 





desired outcome. By tuning the three gains Kp, Ki, and Kd, 
the control designer can meet specific outcome 
requirements. The values for these gains are listed in 


Table 2. 



































Gain Symbol Value 

Proportional | Kp ODO b4 1 

Integral Ki 0.0015 

Derivative Ka 0.0078 
Table 2. PID Controller Gains 


37 


Cc. QBOT GROUND VEHICLE 
1. Introduction 


As one might expect, the modeling and control of a 


three-wheel ground vehicle is considerably less involved 





than that of a quadrotor. Differential drive robots, like 





the Qbot, have added simplifying features that make them 
easier to model than other types of wheeled UGVs. The Qbot 





has two independently-driven, coaxial wheels and a non- 


powered third wheel that provides stability but freely 








rotates to prevent wheel side slippage. The powered wheels 





provide the forward and reverse motion of the vehicle as 





well as the rotation of the vehicle about its center axis. 








This rotation is due to the difference in speed between the 


two wheels, v and Vref (Figure 15). Another simplification 





right 


is that the Qbot lacks a suspension system and therefore 








its motion be modeled in just the X-Y plane. With a top 





speed of y =0.5 m/s, the Qbot can transverse the very 














Vieft right 
flat lab floor with virtually no vertical movement. 
Therefore, the Qbot (Figure 15) has three degrees of 
freedom, the X and Y position of the vehicle and its 


rotation about the Z-axis. 
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D=0.43 m 


Figure 15: Qbot Important Parameters 


The equations which govern the movement are given by: 








avaee + Vieft cos ra) 
x 
‘ . Viient + VY) . 
P=l\yl= DT ind (16) 
(a) 
V ight i” Vieft 
= L | 
2. Inverse Kinematics 


As was mentioned in the previous’ section, the 
trajectory of the Qbot can be controlled by varying the 


velocities of the wheels. The controllers implemented in 


this thesis accept v,,,and v,, aS inputs. From the first two 
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equations of Equation 16, the term v,,,,+ V,, as well as an 








expression for @ can be determined. 


o- * 2 ae = 
Vright tVigg =2* xX +y =F, (17) 


@ = tan"! (2) (18) 
XxX 


Differentiating Equation 18 yields: 








a2 











x Ey ee 
: : : A right = Vs 
Since the third equation of system (16) states Cae 
then 
_  St-85 
Vriete Vien = bg oa = a (20) 
Resolving equations (17) and (20) yields: 
3 
ye Aith _ +) +0.5L(pe—¥) 
right = +2 oa 
2: (x +y°) 
: (21) 
eee ses (x7 + y°)? 0.5L (jx — x9) 


In the absence of disturbances, knowing the controls v,,,,and 





Viehi enables one to know the trajectory x(t), y(t). 
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IV. REACTIVE MOTION PLANNING 


A. INTRODUCTION 


Motion planning refers to the ability of a mobile, 


autonomous system to plan its motions [17]. One category of 





autonomous motion planning is called reactive motion 





planning. This type of planning enables the vehicle to 


navigate through either a static or dynamically changing 





environment by detecting information about obstacles using 
feedback from on-board sensors. The type of motion planning 


involved in this thesis will be a combination of local 





reactive planning and global deliberate planning. 
Specifically, the ground vehicle will be given an end 
destination, but will need to find its own path through a 
series of static obstacles using a variant of the potential 


field technique. 


B. BASICS 





The potential field method is a simple, yet effective 





approach to reactive navigation. The potential field 


approach was chosen because of its ease of implementation 








as well as for its directness. This is opposed to other 


methods like the Voronoi Diagram method, which keeps the 





robot as far as possible from all obstacles in the 





workspace and has relatively long path lengths. With regard 














to potential fields, the target exhibits an attractive 








potential field, while obstacles have a repulsive potential 








field. The resultant potential field is computed by summing 








the contributions from all the potential fields in the 





environment: 
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U = ieee: >. U seer ( 22 ) 


F=-VU (23) 


The robot movement direction is then inclined to the 





direction of the local force [18]. 


Cc. VECTOR FIELD 


This thesis uses a variation of the potential field 





method called the vector field method. Like the potential 








field method, the vector field method uses a virtual force 





field to direct the robot. This field is based on the 





robot’s sensory perception of the environment and it is the 





weighted vector summation of the force field that gives the 


correct vehicle heading. The repulsive force is defined by: 





F.,, i = —w°V, 
fige= Pes (24) 
_|_min(dy.d,) 
di, 


Here V, represents a position vector that gives the range 


data (d,) obtained by i” infrared sensor. F 


obs,i 


is the 
repulsive force that 1s associated with V. The term d,, 
represents the maximum range of the IR sensors. 


Prior to combining these forces to find the 
appropriate direction vector, each force is converted to a 


unit force given by: 
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nH ‘ar 
Ta ~ J 
tar 
ae | 
b. 
obs = ; ( 2 5 ) 
obs 
F=Woy.P oy. + W 


tar~ tar 


In these equations, Fis the Unit etbractive. force 


=> A 


that is linked to the target position vector, V,,. F,, 18 
the unit repulsive foree, The weights, w,, and w,., are used 
to regulate the unit forces to produce the correct 


direction vector F. The robot follows F for collision free 





navigation. As will be explained in the last chapter, the 





experimental trials revealed that there are many sources 


for error involved in using this method ranging from sensor 





error to problems that arise based simply on the geometry 








of the obstacle field, like the local minimum problem. 


This problem occurs when the robot becomes “stuck” after 


finding the local minimum of the environment rather than 





the global minimum. 


Local minimum 







4 
i se 
3 
3 5 Global minimum 
x (goal) 
0 
8 
6 
4 
x/ 
~ 
ae 
~ 7 
Eee a 2 
——— 4 
2% Y/m 
’ 6 
Figure 16: Local Minimum Problem. From [19]. 
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In Figure 16, a robot might follow the force vector 








into the horseshoe-shaped obstacle and become stuck in the 


local minimum there. There are numerous strategies and 





various bug algorithms like wall following that can be used 
to combat this problem, however, this thesis will not 


involve complicated geometries with local minima as this 











will only increase the amount of time required for the 
robot to traverse the environment using reactive 


navigation. Additionally, circular obstacles will be used, 





which also helps to reduce errors in the reactive 


navigation portion of the thesis. 
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V. IMAGERY ANALYSIS 


A. INTRODUCTION 


Computer vision and image processing are very active 
areas of research with regard to autonomous systems and 
environment navigation. Computer vision navigation relies 
on image sensors to provide data to the system about its 
external surroundings. This process is not always as simple 


as it sounds as images must first be acquired, processed, 





and analyzed in order to find out any useful information. 
This thesis will make use of the image acquisition process 
known as the pinhole camera model. Moreover, the physical 


camera will be a downward-looking, wide-angle camera. 


A. IMAGE THRESHOLDING AND CENTROID CALCULATION 


Image thresholding is a very simple form of image 
segmentation. From a greyscale image, thresholding can be 
used to make a binary image. The most important parameter 
with regard to thresholding is the threshold value. The 
process by which thresholding is accomplished in this 


thesis is as follows. First, an initial threshold value of 





60% is chosen. The image (Figure 17) is then segmented into 


object and background pixels. If a pixel is brighter than 





the threshold limit it is marked as an object pixel and it 





is given a value of “1”. Conversely, if a pixel is darker 





than the threshold, it is marked as a background pixel and 





it is given a value of “0”. The binary image (Figure 18) is 





then based off these numbers. In this thesis, the obstacles 


are white cylinders and the floor is black foam so the 





image thresholding was accurate and non-problematic. In 


order to find the centroid of the obstacles, the white 
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pixels, labeled “1,” are grouped together as objects if a 


certain number of similarly labeled pixels are adjacent to 





one another. Then the means of the x and y coordinates of 
this object are calculated and the new point (Xmean,Ymean) iS 


called the centroid. 





Figure 17: Black and White Image 


Image Thresholding and Centroid Calculation 





Figure 18: Image Thresholding and Centroid Calculation 





Cc. PINHOLE MODEL AND COORDINATE FRAMES 


The pinhole camera is a popular way to model a camera 
for computer vision processes. As is shown below, the model 
(Figure 19) involves the use of three coordinate systems: 


the image, camera, and world coordinate systems. For the 











pinhole camera depicted in Figure 14, the plane R is the 


image plane and the location Oc is called the optical 





center. The distance between R and Oc is called the focal 
length, £ The location 0O;, where the optical axis 


intersects R, is called the principal point. The coordinate 





system (O;, Xi, Yr) is considered the image coordinate system 





and the coordinate system (Oc, Xc, Yc, 4c) iS called the 


camera coordinate system. 





In order to relate a position to a global frame, a 


world coordinate system (Ow, Xw, Yw, Zw) must also be defined. 





Below is a figure representing these coordinate systems. 





Figure 19: Pinhole Camera Model. From [20]. 
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Also the points X and x are defined with coordinates 


(x.,y¥..Zz.) and (%,y,) respectively. From similar triangles, one 





can see that in the far field where z>>f a relationship 
exists between a point in the image frame and a point in 


the camera frame. 





























x,=-f— 
; (26) 
y,=—-f *< 
Xe 
Therefore, if x=PsX then in homogenous coordinates: 
[f 0 0 O] 
0 0 O 
P= f 
0 0 0 0 
[O O -l1 fF] 
_ 7 Ix. eG x, (2 y ) 
f 0 O O}f x, pe, Pe or 
0 0 O 
P, — Z Ye = HV. — I. = fre 
00 0 Offz, 0 Ie: ae 
|O O -1 fii 1 FHz, 0 0 
= 1 | = | 
D. TRANSFORMATIONS 


Part of the challenge of using a camera aS a sensor is 
the fact that there are multiple coordinate systems that 


must be resolved in terms of one another. This will be 





accomplished through a rotation and translation matrix. In 
this thesis, the shorthand notation cO is used to represent 


cos(@)and similarly s@ to mean sin(@). To transform vectors 
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from one coordinate frame to another, the direction cosine 





matrix (DCM) is used. The rotation matrices about the x, y, 


and z axis are: 


(28) 








(30) 








Additionally, a translation term d can be added to 








allow for translational movement from one frame to another. 





R, Ry R, d. 
Ry Ry Ry <d 
R(60,d) = 21 22 23 y (31) 
R;, Ry R;; d. 
0 0O O 1 


N“ 


Given an object “s” with only coordinates x and y (z=0), 


with the equations above one is able to compute the 








projection of that object in the world frame on to the 


image frame. 


°p, = P,R(O,"d,.)" p, (32) 
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w'x, if 0 0 0 RT 0 R P ee i s 
EG: oo Oe Oe |S. 


1 0 
w 0 0 -l1 f ? 


The term w in this equation is a scaling factor. While 


Equation 33 presents a useful relationship, in this thesis 





we are interested in finding the world coordinates of an 


object based off its image coordinates. Defining M as: 


f-0' & © 
M=|0 f 0 O|R,(@,"d,) 
ONO) SF 
"XW! & 


"yw! -M"! ay 


' 


w 1 


w'=M '(3,1)°x,+M 1(3,2)°y, + M (3,3) 


(34) 


Ss 





In Equation 34, R,(0,"d,) is R(@,"d,) without the third 








column. Equation 33 is only invertible when the camera is 
high in the sky. The notation in the scaling factor 
equation to find w’ refers to M'(i,j) where i is row and j 


is the column of the M?’ matrix. In this thesis, the objects 





in the world frame will be the obstacles located on the 





ground plane where z=0 and the camera will be stationary 


above this plane at a height of roughly 1.5 meters. 
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VI. OPTIMAL CONTROL BY DIRECT METHOD 


A. INTRODUCTION 


The optimal control problem this thesis addresses is 


to guide a ground vehicle from an initial state to some 





final state with constraints imposed on both the states and 


the controls. Ideally, the routine that is used should be 








capable of updating itself multiple times over the course 


of the trajectory to mitigate disturbances and unmodeled 





motor dynamics. Traditional indirect methods are not able 





to handle this problem in real-time, leaving the 


alternative direct method as the ideal choice to formulate 





our vehicle path. 


The main idea behind the direct methods technique is 


the use of a finite set of variables to arrive at an 





optimal or quasi-optimal solution. This approach involves 








using a function to approximate the states and controls and 
a function to represent the cost of the process. The 
coefficients of these function approximations are used as 


variables in the optimization problem, which becomes a 





nonlinear optimization problem rather than the more 





challenging boundary-value problem. A great amount of 
research has been done with regard to this type of 
optimization and there are a wide variety of optimization 


software packages available that can be used to. solve 





problems somewhat quickly [21]. However, many of these 
methods, such as the direct collocation method, rely on 
thousands of variables and constraints, which add 


Significantly to computational time and cost. 
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The method that is used in this thesis research is the 





direct method of calculus of variations exploiting the 





inverse dynamics of a vehicle in the virtual domain 





(IDVD) [22]. As the name suggests, one of the key features 





of this method is that it utilizes the inverse dynamics 


(kinematics in our case) of th vehicl described by a 





system of differentially “flat” equations E22 I + 
Consequently, all optimization occurs in the output space 


as opposed to the control space. This feature, when 





examined in a virtual domain, allows for fast prototyping 


of optimal trajectories. Additionally, since the IDVD 











method uses so few varied parameters, the computational 








requirements that must be met are significantly diminished. 
The method is also easy to modify and code, which gives the 
operator more freedom with regard mission scenarios. 

B. IDVD METHOD COMPONENTS 


1. Reference Trajectory 


An important feature of the IDVD method is the fact 





that the reference trajectory is independent of any time 
constraints. This is accomplished by creating a path from 
mathematical functions for each Cartesian coordinate using 
a virtual arc as the independent variable. The 


equations for the x-coordinate are shown below. 
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1 max(1,k —2))!c* 
X(T) = Yai kl » 
k=0 . 


oy Me (max(1,k —2))!7*t 
4) = Qe GE) 





, (35) 
X(t) = Sag 
k=2 


X(T) = Sk —2)a,t°° 


The y coordinate is found through the same procedure. 


As seen, the simplicity of the Qbot allows for the use of 


N“ Ww 





simple polynomials of order “n There are many different 
options for the choice of reference function and the type 
depends primarily on the shape of the trajectory that is 
desired. Curvy trajectories might be better approximated by 


a combination of lower-order polynomials and trigonometric 











terms. The degree of these polynomials is based on the 





number of boundary conditions specified by the problem and 


the coefficients a, must be determined algebraically. For 











example, the higher the time derivative of a vehicle at its 
beginning and final coordinates, the higher the order of 


the polynomial. The minimum polynomial degree is given by 
n=d,+d,+1 (36) 


d,andd, are the maximum orders of the time derivatives of 


the vehicle at the initial and end points. To clarify, if 


one were to specify the first and second time derivatives 





of the initial and final points of the vehicle, the order 


of the reference polynomial would be n=2+2+1=5. 











However, to ensure there is flexibility with regard to 








the shape of the trajectory as well as smooth transitioning 


at the initial and final points, the third order 
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derivative, or jerk, are specified as an additional varied 








parameter. This amounts to three initial and three final 


conditions which according to Equation (36) would yield a 





seventh order polynomial. To solve for the coefficients of 





these polynomials, the following matrix equation can be set 


up. 


























10 0 0 0 0 0 Oo. 2 
01 O 0 0 0 0 0 Ay | | % 
00 1 0 0 0 0 0 a, | | % 
00 O 1 0 0 0 0 Ay} |X 
1 2 1 3 1 4 1 5 1 é 1 7 \* a,3 ro Xo 

1 ct T T T T T T = (37) 
BS SE GA rhs Bek, AD eet Vise 
Od a | Ey 7 u oe | ec u a as ie 
2, 3 4 5 6 dee Nee 

2 3 4 5 
0 O 1 Ty T, T, Ty T, la,, | lL, | 
OO 90" 4b 2a. are ae See 


The matrix equation for the y coordinates looks similar to 


the one below and has been omitted. In these equations 4%, X 


1%, X,,k%,,k#,are given while x and *%, are considered to be 


varied parameters. 


2. Speed Factor 


As mentioned above, one of the advantages of the IDVD 
method is the ability to decouple space and time. This is 
why a virtual arc is used as an argument as opposed to 


time. This virtual arc calls for a virtual speed. 


de 


Dime 
dt 
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In order to maintain simplicity, the virtual speed or speed 


factor is approximated with the same method that was used 





for the reference trajectory. 


n 


qe Sia, c! (max(1,k —2))! 











40 k! 
Aa < 1k-2))! 
da _ yey (max(l,k=2) ie 
dt ‘a (k-1)! 
2 n 
eee ane 
dt = 


If we set the boundary conditions to unity such that 





A,=A,=1, the first derivatives to zero, and use the second 
derivatives as varied parameters, the matrix form can be 


determined as: 
































[1 O 0 0 0 0 | a 

0 

0 0 0 0 0 Ayo me 

0 O 1 0 0 0 a), . 
i {4 |e Ay (40) 

1 T, ae ce t, oe ae 

BE GP Pe 80 Flagg 

ie i 1 ' 

QO 1 % Ge te ae Aas Ay 

3 ; 4 ; | a5 | A " 

| 9 0 1 Tf T Tt, | Es ae 
3. Mapping from the Virtual to the Time Domain 


Now that our candidate trajectory is defined in the 
virtual domain by Equation (35) (with coefficients found 
from Equation [37]), we need to produce time stamps or, in 


other words, transfer this trajectory to the physical 





(time) domain. Virtual argument srvaries from 0 to T; 


(another varied parameter), and computations are performed 
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at two hundred points along this virtual arc with the 





constant AT. In between two nodes, ft, and Tf 


j wie. che “vehie Le 





travels: 
As =X -%,) +O - 9)” (41) 
For this interval, we also know 4, and 4,,,. Therefore, w 


also know that: 


AT 
At =t..,-t.= (42) 
ged Kiss +A, 
2 
4. Cost Function 


The cost function is constructed for usage with the 
unconstrained optimization Matlab routine, fminsearch. This 
function consists of a performance index with a minimum 


time or fixed time and also several penalty terms. Two 





terms penalize for violations of the Vmax constraint: 


2 
nee Vien, ) = Via 








max| 0, 
View 
5 (43) 
max (V sent j ) a Ves 
max| 0,— 
Vege 
Also, there is penalty turn for running into obstacles: 
D-—min min d,, 
k aed 
max| 0, / (44) 


D 
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In this last term, dij is the distance at node j to obstacle 


k (index k runs through all K obstacles and j runs through 





all nodes), and D is the minimum distance. 


Once a candidate trajectory is computed there is a 
cost attached to it. The fminsearch function then attempts 


to minimize this cost function by varying seven problem 


parameters XoX p> Vor Ver lor po Tp « To be more precise, it varies 


T Tp» PorPpotortp ts where 


Xy = Ty COS Py 
Yo = TF SIN Po 


x, =T, COS P, 


(45) 


Y, =T, SiN, 
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VII. LAB IMPLEMENTATION 


A. INTRODUCTION 


One of the main goals for this research is to provide 


the framework for a method of rapidly calculating a vehicle 








trajectory based on global information about the 
environment surrounding the vehicle. In this thesis, the 


global information is provided by a visual camera mounted 





on an aerial vehicle. The trajectory generation algorithm 





that is described above would certainly support information 


provided by other methods and sensors. 


As a validation of the trajectory generation results, 


a test case involving the same vehicle and the same 





obstacle course is performed using reactive navigation. 
This method relies on sensor feedback from five infrared 
sensors mounted on the front half of the Qbot to create a 


vector potential field as discussed previously. 


B. SIMULATION RESULTS 


Due to the short duration of the flight and the fact 





that the obstacles are stationary in the test situations, 





the optimal trajectory is only calculated once on the 
ground station computer and then uploaded to the Qbot via a 


wireless ad-hoc network. The developed algorithm could most 





certainly be applied to a test where the optimal trajectory 





is recalculated during vehicle operation. This would help 
account for controller errors, disturbances, and dynamic 


obstacles. 


First, the performance of the Qball as a platform for 








the downward-looking camera must be evaluated. At the start 
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of the trajectory generation test, the Qball-X4 is sent to 
the position directly above the Qbot to acquire a photo. 
The plots of the commanded Qball trajectory parameters 


versus the actual parameters are given below: 


Commanded vs. Experimental Trajectory Qball: Commanded Versus Actual Location in Z direction 
1.45 T ra T 7 1.5 . : - ; : 


x Actual 
; a Commanded 


iL | Pe 
h Actual as if a mle \ i 
0.8b Commanded | Fee ‘ ans f 
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One of the issues that was discovered during the course of 
this thesis was that the various controllers developed by 
Quanser for the vehicle X-Y position and heights were not 
optimized. As is evident from the graphs, there is a 


Significant amount of error in the tracking of the 





commanded trajectory. Since the xX-Y position of the 





quadrotor is changed via modifications in roll and pitch, a 





steady downward angle is difficult to achieve. In order to 
take accurate pictures with reproducible image analysis 


results, a stable platform is required. Additionally, the 
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Qball-X4 has only one processor on-board the vehicle. 





Simultaneous image processing and vehicle control proved to 


be too computationally intensive for the on-board computer. 








As a result, when calculating the optimal collision-free 





trajectory, the six obstacle positions were taken from 


measurements from the OptiTrack system. 


As a benchmark to compare our optimal trajectory 


results against, the scenario was first run using only 





reactive navigation. Table 1 is a list of the initial 


coordinates for both the robot and the obstacle course 





















































(Figure 20). 
X Position (m) Y Position (m) 
Intel Point Oe Aor 1.3417 
Final Point -0.024 =O. 1382 
Obstacle 1 -0.5084 =02030 
Obstacle 2 =0.3457 0.22999 
Obstacle 3 -0.5174 O27239 
Obstacle 4 0.3056 -0.4343 
Obstacle 5 0.4221 0.1804 
Obstacle 6 O32 O.27832 
Pe blke- us Initial Robot and Obstacle Locations 
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Figure 20: 


The 


navigation was 


QObot 


and the 


collisions 


Obstacle Locations 





0.8 


O.6- 


O.4- 


O.2- 


Crossrange, m 


oat 


O.6- 








time 





wit 


1 
0.4 0.2 a o.2 


Downrange, rn 


Obstacle Locations 


to 
70.3 seconds. 
could not 


th max 


complete 


the course 


xecut 





using reactive 


Multiple tests were performed 


th course without 








wheel 


speed greater 


than 0.05 m/s. 


Figure 20 shows a plot of the Qbot trajectory as well as 


the IR sensor returns: 
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Reactive Navigation Trajectory 
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Figure 21: Reactive Navigation Plot 


As is evident from the plot, the trajectory, while 
collision-free, is certainly not optimal, and takes too 


much time to complete. 


As a comparison, Figure 25 presents the result of the 
collaborative scenario where the Qbot utilizes the global 
information provided by the Qball. The trajectory only 
takes 26.4232 CPU seconds to compute and 15 seconds to 
complete. Figures 23-25 show the time histories of A, 
heading, yaw rate, speed, and controls. In these plots, the 
last points for the heading and yaw rate are irrelevant 


because the speed is zero. 
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This solution is then fed to the Qbot controller (we 








use the interpl() function to produce a vector of control 





inputs that are evenly spaced in the time domain) and the 





Qbot tracks it perfectly. 
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The trajectory shown in Figure 25 seems obvious but it 
should be emphasized that it also contains feasible 


controls and therefore is ready to be tracked. 





A second scenario that may be deemed more 


sophisticated is shown in Figure 26. 










Ic 
Oo FC 

Qbot trajectory 
A Obstacles 
\ — > Velocity 








0.8- 


0.4, 


0.2- 


Crossrange (y-axis), m 


0.2, 


0.4 


0.66 














Downrange (x-axis), m 


Figure 26: IDVD Scenario #2 Trajectory 
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Figures 27-29 show Ay heading, yaw rate, speed, and 
controls versus time. The trajectory here is curvier but 


the rest is the same-all the constraints are satisfied. In 





this case, the native reactive navigation Qbot controller 





had even more difficulty finding a way around. This example 


highlights the advantage of the proposed approach. 
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VIII. CONCLUSION AND FUTURE WORK RECOMMENDATION 


A. CONCLUSIONS 


The following conclusions have been made through the 


course of this thesis research: 


e An architecture for the control of multiple, 


varied unmanned vehicles has been developed and 





tested. This system allows for control via a 


single base station with minimal user input. 





e The inverse dynamics in the virtual 


based trajectory generator is able to 





domain 


generate 


an accurate, feasible trajectory that allows 


for collision-free operation. This 


is faster and more accurate than 


navigation methods. 


traject 


react 


Lory 





Live 


e The framework for a system that recalculates 








trajectories despit disturbances, 





the trajectory at multiple points along a path 
has been developed. This would allow for safe 


controller 


inconsistencies, and dynamic obstacles. 


e The native Qball LOR controllers 
accurately track trajectories. 
controllers would enable more options 
regard to leader-follower missions 


do 


not 


Tuned 


with 


involving 


both the quadrotor and ground vehicle. 


e The Logitech camera on board both the Qball 


and Obot has a slow rate of 


response. 


This makes image processing very challenging 





and requires very slow vehicle velocities 


order to be useful. 
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in 





B. RECOMMENDATIONS 


The following are recommendations for future work 


involving the Qball-X4 and Qbot unmanned systems: 


e Improve the native Quanser controllers onboard 
the Qball-X4 quadrotor. This would allow the 
Qball to track trajectories with more precision 





and allow for a more stable platform for image 


capture. 


e Utilize a second processor onboard the Qball-X4 


for image capture and processing. 


free up the current embedded processor 





control the vehicle. 





This would 
to 


e Use an improved reference function that would 


allow for more freedom with 


trajectories. This could include 





trigonometric functions. 


e Enable trajectory updates onboard 











through the use of compiled C++ code. 


e Incorporate more vehicles that must 
through more complicated obstacle fields. 


would take pictures at various locations 





well as multiple ground vehicles 





rendezvous at a certain point. 
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regard 


that uses lower-order polynomials 


to 


function 


or 


the Qbot 


navigate 


as 


This 
could include adding multiple quadrotors that 





that must 


[AS] 
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APPENDIX 


Matlab Code 





DMlopt .m-This code uses the function fminsearch and DMlfun 


to simulate the Simulink model DM2 in order to create an 





optimized trajectory that minimizes the specified costs. 


close all, clear all, cle 

warning off 

$% Obstacles 

& Obs=([1 2-07; -—2 3-07 2 3°07 4-4 0]; 

% Obs=[441.6 189.7 312.4 =—512.0 -—593.8 -405.17 0.25 0.25 0.25 0.25 0.25 
0.25; -382.5 82.6 544.1 463.0 -206.1 -543.1]'/100; 

% Obs=Obs(:,[1 3 2]); 

Obs=[-0.5084 -0.3457 -0.5174 0.3056 0.4221 0.3629; -0.2830 0.2553 
0.7289 -0.4343 0.18039 0.7832; 0.19 0.19 0.19 0.19 0.19 0.19]'; 
Obs (:,1)=-Obs(:,1); 

sObs=Obs(:,[1 3 2]); 


oo 


%% Initial guesses for varied parameter 








x0=[0.084 % Lambda dprime 0 
-0.0071 % Lambda dprime f 
0.0014 % XOppp 
0.0012 % XOpppAngle 
-0.0012 % Xfppp 
0.0001 % XfpppxX0pppAngle 
18/1000]; % tau f 


t = cputime; 
options=optimset ('TolFun',1le-3,'TolX',1le- 

3, 'Display','iter', 'MaxFunEvals',200); 

[xv, fval,exitflag]=fminsearch (@DMlfun, x0,options) ; 
time elapsed = cputime-t 

disp([x0 xv]) 
xO0=xv; 

lam0O_2pr =x0 (1); 
lamf 2pr =x0 (2); 
XO tpl prime =x0(3); 
xO_tpl _primeA =x0 (4); 
Xf tpl _ prime =x0(5); 
Xf tpl _ primeA =x0 (6); 
tauf =x0(7); 
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DMlfun.m-This function provides the varied parameters and 


Simulates the DM2 Simulink Model. 


function f = DMlfun ( 
lam0_2pr =X 
lamf 2pr =X 
XO tpl prime =x 
XO tpl _primeA =x 
Xf tpl prime =x 
Xf tpl_primeA = 
tauf =X ( 





sim('DM2',[0 200], opt 
f=yout (length (yout) ); 

Compute_Controls_pos.m- This code computes the commands for 
the Qbot ground vehicle and interpolates them so that they 


are provided to the controller at the right frequency. 


fo) 


% Controller speed 
ctrl _ t_step = 0.1; 


% Run Simulation to get data 


Ssim('DM2', [0 200]) 
sim('DM3', [0 200]) 
[m,n] = size(a); 


t_end = a(m,1); 
t = O:ctrl_t_step:t_end; 


% Setup Variables 
tau = a(:,1); 


& theta = a(:,3); 
x = a(:,3); 

y a(:,4); 
Z=a(i,5)7 

x vel = a(:,6); 

y vel = a(:,7); 

z vel = a(:,8); 

x accel = a(:,9); 
y accel = a(:,10); 
z accel = a(:,11); 


Vright = a(:,13); 
lambda = ) 


| 
ie) 
s 
hb 
Ds 


Vleft_mm=interpl (tau, Vleft, linspace (0,15,1+t_end/0.1),'spline')*1000; 
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Vright_mm=interpl (tau, Vright, linspace(0,15,1+t_end/0.1),'spline')*1000; 


Vleft_mm=[t; Vleft_mm]'; 
Vright_mm=[t; Vright_mm]'; 


% Vleft_mm=int1l6(Vleft_interp*1000); 
% Vright_mm=int1l6(Vright_interp*1000); 


figure 

plot(a(:,1),Vleft); 

hold on; 

title('Wheel Velocity Commands') 
plot(a(:,1),Vright,'r'); 

plot([a(1,1) a(end,1)],.5*[1 1], '--r') 

legend ("¥_ {leftt}',"¥_{¥ight}’, "Constraints", 0) 
plot([a(1,1) a(end,1)],-.5*[1 1],'--r') 
xlabel('Time, s'), ylabel('Speed, mm/s') 








%% Setup data for use in controller 
% Setup a series of commands for the first waypoint 
start = 0; %Start time for maneuver 

= ttt_start; 





t_beginning = O:ctrl_t_step:t_start-ctrl_t_step; 
z comp = ones(1,length(t_beginning) ); 


WU 


comp = [t_beginning' t_beginning';t' 





ie tol; 
% x command = [t_beginning' x(1)*z_ comp';t' x']; 
% y_ command = [t_beginning' y(1)*z_ comp';t' y']; 
% z command = [t_beginning' z(1)*z comp';t' z']; 


2 oO 


3% plot vehicle trajectory, superimpose obstacles 
figure 
title('Position of Qbot') 
plot (x(1),y(1),'dm'), hold, plot(x(end),y(end),'rs'), plot(x,y,'b-") 
xlabel('Downrange, m'), ylabel('Crossrange, m') 
sdaspect([1 11]) % set aspect ratio so circles appear as circles 
hold on; axis equal 
plot (Obs(:,1),Obs(:,2),'LineStyle','none','Marker','%*',... 
"MarkerFaceColor','k', 'MarkerEdgeColor','k', 'MarkerSize',10) 
legend('IC', 'FC', 'Qbot Trajectory', 'Obstacles',0) 
% Plot the safety distance of 0.25m around each of the obstacles. 
for i=1:length (Obs) 
rectangle('Position', [Obs(i,1)-0.25 Obs(i,2)-0.25 0.5 0.5],... 
"Curvature', [1 1],'EdgeColor','k"') 











end 


6% Interpolate data 
% Interpolate data between points at the same frequency the controller 
6 runs at. 
Sphi = interpl(tau,phi,t,'pchip'); 
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theta = interpl(tau,theta,t,'pchip'); 








x = interpl (tau,x,t,'pchip'); 

y = interpl(tau,y,t,'pchip'); 

z = interpl(tau,z,t,'pchip'); 

x vel = interpl(tau,x _vel,t,'pchip'); 
y_vel = interpl(tau,y vel,t,'pchip'); 

z vel = interpl(tau,z vel,t,'pchip'); 

x accel = interpl(tau,x_accel,t,'pchip'); 
y_accel = interpl(tau,y accel,t,'pchip'); 
z accel = interpl(tau,z accel,t,'pchip'); 
figure 


plot (tau, lambda) 
xlabel('\tau') 
ylabel ('\lambda") 


oF 


ro) 





Controller_design.m-— This code takes into account the Qball 
vehicle parameters in order to calculate the controller 


parameters. 


& This file contains all the controller parameters and LOR gains for 
the 
% Qball. 


% PITCH and ROLL 


wnom = 15; 
L = 0.2; 
w = wnom; 
K = 120; 
J = 0.03; 


Jyaw = 0.04; 
CLimit = 0.025; 


M= 1.4; 

g = 9.8; 

Am = [0 1 0 
0 0 2*K*L/T 
0 0 -w]; 

Bm = [0 0 w]'; 


Aobs = Am' ; 
Bobs = eye(3); 
Qobs = diag([.001 10000 .01]); 


Robs = diag([ 111 ])*1; 

Kobs = lqr(Aobs, Bobs, Qobs, Robs) 
Kobs = Kobs'; 

Aobs = Aobs'-Kobs*Bobs'; 

eig (Aobs) 

Bobs = [Bm Kobs] 

Cobs = eye (3) 
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% augment with integrator 
Ai = [Am [0 0 0 ]' 


1000]; 
Bi = [Bm' O]'; 
Ci = eye(A4); 
Di = [0 000 j'; 





Q = diag([100 0 22000 10]); 
R = 30000; 


ki = lgqr(Ai,Bi,Q,R); 

rp_eig = eig(Ai-Bi*ki); 

fprintf (PE RRKKRERAREAERE REE ERE KEKE RE KE KERERREK ER EKER ER KK EK \n'); 
fprintf('ROLL, PITCH DESIGN \n"'); 

fprintf( 'P = %5.3f D = %5.3f Actuator = %5.3f I = %$5.3f \n\n',ki(1), 
ki(2),ki(3),ki(4)); 

for i= 1:4 

fprintf(' %5.3f + %5.3f i \n ',real(rp_eig(i)), imag(rp_eig(i))); 
end; 














SPOSITION CONTROLLER (C2) 








6 XZ travel 
tlimit = 5*pi/180; %Smax pitch cmd radians 
Stlimit = 15*pi/180; %Smax pitch cmd radians 
vlimit = 0.3; % max speed cmd in m/sec 
Svlimit = 0.5; % max speed cmd in m/sec 
Tau_theta = 1/7; % closed loop time constant for pitch response 
wt =1/Tau_theta; closed loop theta bandwidth 
ke. = Ly 
a= [0100 
00g 0 
0 0 -wt 0 
1000]; 
b= [0 0 wt 0 ]'; 
q = diag([ 5 2 0 0.1])); 
sq = Giag([ 5 2 0 0.1)]); 
Sr = 50; 
r = 50; 
k = lqr(a,b,q,r); 
ac = a-b*k; 
xy _eig = eig(a-b*k); 
Kp = k(1); 
Kd k(2); 
Ki = k(4); 
Kw = k(3); 
forintf('\n\n X Y Design \n'); 
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fprintf( 'P = %5.3f D = %5.3f Actuator = %5.3f I = %5.3f \n\n',k(1), 
k(2),k(3),k(4)); 

for i= 1:4 

fprintf(' %5.3f + 65.3f i \n ',real(xy_eig(i)), imag(xy_eig(i))); 
end; 


ole 


Z axis w actuator 


% vlimith = 0.1; 


% Amh = [0 1 0 

% 0 0 4*K/M 
% 0 0 -wl; 

6 Bmh = [0 0 w]'; 
6 Cmh = eye(3); 


Dm = [OOO 153 


6 S$ augment with integrator 
% Ahi = [Amh [0 0 0 ]' 








% i 0°O 0 IF 

% Bhi = [Bmh' 0]'; 

& Chi = eye(4); 

%® Dhi = [0 000 ]'; 


&® Q = diag([30 .8 12000 10]); 

%® Q = diag([0 0 000 100]); 

% R = 2000000; 

% kh = lqr(Ahi,Bhi,Q,R); 

% Ep 1g = e1¢(Al-Bi*kh); 

z fprintf (PE RRKKKE KAR KKKERK KKK KKK KER KKEKKK KKK KK KEKKEKKKK KK KKK KK \n')> 
% fprintf('Z DESIGN \n'); 

% fprintf( 'P = $5.3f D = %5.3f Actuator = %5.3f I = %5.3f \n\n',kh(1), 
kh (2),kh(3),kh(4)); 
% for i=1: 
& fprintf(' 
% end; 

% Kph = kh 
% Kdh 
% Kwh = kh 
% Kih 











ds 


So.3f + $5.3f 2 \m “;feal(rp eig(i)), imag(epeig(i))); 


ll 
= 
> y 


ll 
= 
23 


ae 


Z axis without actuator 


vlimith = 0.1; 


Amh = [0 1 
00 ] 
Bmh = [0 4*K/M]'; 


Cmh = [1 O]; 
Dmh 0; 





fo) 


% augment with integrator 
Aih = [Amh [0 0 ]' 

1 00]; 
Bih = [Bmh' 0]'; 
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Cih eye (3); 
Dih = [0 0 O]'; 


Q = diag([1 0 50]); 

R = 5000000; 

kh = lgr(Aih,Bih,Q,R); 

h eig = eig(Aih-Bih*kh) ; 

fprintf (ERR RERA RRA KKK KE EKER EK EEREKK EEK KE EK EKRK RE KER KEK KEK AA) 
fprintf('Z DESIGN \n'); 

fprintf( 'P = %5.3f D = $5.3f I = $5.3f \n\n',kh(1), kh(2),kh(3)); 
for i= 1:3 

fprintf(' %5.3f + %5.3f i \n ',real(h_eig(i)), imag(h_eig(i))); 
end; 

Kph = kh(1); 

Kdh kh (2); 

Kwh = 0; 

Kih = kh(3); 








, 








% yaw axis 


Ky = 4; 
Jy = 0.032; 
Amy = [0 1 
00 |; 
Bmy = [0 4*Ky/Jy]'; 
Cmy = eye(2); 
Dmy = [0;0]; 
Qy = diag([1 0.1]); 
Ry = 1000; 


ky = lqr (Amy, Bmy, Qy,Ry); 
h_eigy = eig(Amy-Bmy*ky) ; 
Kpyaw = ky(1); 

Kdyaw = ky(2); 

Bih = [Bih, [0 1 0]']; 

Dih [Dih, [0 0 Oj']; 











Filter_design.m- This code is run when Qball-X4 model is 
first started in order to find the complimentary filter 


coefficients. 


t=10; 

Ss = tis"); 

Gg = t%*2*s/(t*st1)%*2 

Gi (2*t*st1)/(t*st1)%*2 
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